package controllers;


public class PI {
  private PIParameters innerContParameters;

  private double innerI; // Integrator state
  
  private double innerv; // Desired control signal
  private double innere; // Current control error
  
  public PI(String name) {
	  	innerContParameters = new PIParameters();
		innerContParameters.K = 1.4;
		innerContParameters.Ti = 0.25;
		innerContParameters.Tr = 10.0;
		innerContParameters.Beta = 1.0;
		innerContParameters.H = 0.01;
		innerContParameters.IntegratorOn = false;
		setParameters(innerContParameters);
		this.innerI = 0.0;
		this.innerv = 0.0;
		this.innere = 0.0;
	  
  }

  public synchronized double calculateOutput(double y, double yref) {
	  this.innere = yref - y;
	  this.innerv = innerContParameters.K * (innerContParameters.Beta * yref - y) + innerI; 
	  return this.innerv;
  }

  public synchronized void updateState(double u) {
	  if (innerContParameters.IntegratorOn) {
		  innerI = innerI + (innerContParameters.K * innerContParameters.H / innerContParameters.Ti) * innere + (innerContParameters.H / innerContParameters.Tr) * (u - innerv);
	  } else {
		  innerI = 0.0;
	  }
  }

  public synchronized long getHMillis() {
  	return (long)(innerContParameters.H * 1000.0);
  }

  public synchronized void setParameters(PIParameters newParameters) {
  	innerContParameters = (PIParameters)newParameters.clone();
	if (!innerContParameters.IntegratorOn) {
		innerI = 0.0;
	}
  }

public PIParameters getParameters() {
	return innerContParameters;
}
}
   
